#include "move.h"
#include "dc_motor_control.h"
#include "motor_control.h"

// 前进
void Move_Forward(float32_t speed) {
    SetMotor1Direction(FORWARD); SetMotor1Speed((uint16_t)(speed * 0xFFFF));
    SetMotor2Direction(FORWARD); SetMotor2Speed((uint16_t)(speed * 0xFFFF));
    SetMotor3Direction(FORWARD); SetMotor3Speed((uint16_t)(speed * 0xFFFF));
    SetMotor4Direction(FORWARD); SetMotor4Speed((uint16_t)(speed * 0xFFFF));
}

// 后退
void Move_Backward(float32_t speed) {
    SetMotor1Direction(BACKWARD); SetMotor1Speed((uint16_t)(speed * 0xFFFF));
    SetMotor2Direction(BACKWARD); SetMotor2Speed((uint16_t)(speed * 0xFFFF));
    SetMotor3Direction(BACKWARD); SetMotor3Speed((uint16_t)(speed * 0xFFFF));
    SetMotor4Direction(BACKWARD); SetMotor4Speed((uint16_t)(speed * 0xFFFF));
}

// 左移（麦克纳姆轮横向移动）
void Move_Left(float32_t speed) {
    SetMotor1Direction(BACKWARD); SetMotor1Speed((uint16_t)(speed * 0xFFFF));
    SetMotor2Direction(FORWARD);  SetMotor2Speed((uint16_t)(speed * 0xFFFF));
    SetMotor3Direction(FORWARD);  SetMotor3Speed((uint16_t)(speed * 0xFFFF));
    SetMotor4Direction(BACKWARD); SetMotor4Speed((uint16_t)(speed * 0xFFFF));
}

// 右移（麦克纳姆轮横向移动）
void Move_Right(float32_t speed) {
    SetMotor1Direction(FORWARD);  SetMotor1Speed((uint16_t)(speed * 0xFFFF));
    SetMotor2Direction(BACKWARD); SetMotor2Speed((uint16_t)(speed * 0xFFFF));
    SetMotor3Direction(BACKWARD); SetMotor3Speed((uint16_t)(speed * 0xFFFF));
    SetMotor4Direction(FORWARD);  SetMotor4Speed((uint16_t)(speed * 0xFFFF));
}

// 左转（弯道）
void Move_TurnLeft(float32_t speed) {
    SetMotor1Direction(FORWARD);  SetMotor1Speed((uint16_t)(speed * 0xFFFF));
    SetMotor2Direction(FORWARD);  SetMotor2Speed((uint16_t)(speed * 0xFFFF));
    SetMotor3Direction(BACKWARD); SetMotor3Speed((uint16_t)(speed * 0xFFFF));
    SetMotor4Direction(BACKWARD); SetMotor4Speed((uint16_t)(speed * 0xFFFF));
}

// 右转（弯道）
void Move_TurnRight(float32_t speed) {
    SetMotor1Direction(BACKWARD); SetMotor1Speed((uint16_t)(speed * 0xFFFF));
    SetMotor2Direction(BACKWARD); SetMotor2Speed((uint16_t)(speed * 0xFFFF));
    SetMotor3Direction(FORWARD);  SetMotor3Speed((uint16_t)(speed * 0xFFFF));
    SetMotor4Direction(FORWARD);  SetMotor4Speed((uint16_t)(speed * 0xFFFF));
}

// 原地左旋（所有轮对称反向）
void Move_RotateLeft(float32_t speed) {
    SetMotor1Direction(BACKWARD); SetMotor1Speed((uint16_t)(speed * 0xFFFF));
    SetMotor2Direction(BACKWARD); SetMotor2Speed((uint16_t)(speed * 0xFFFF));
    SetMotor3Direction(FORWARD);  SetMotor3Speed((uint16_t)(speed * 0xFFFF));
    SetMotor4Direction(FORWARD);  SetMotor4Speed((uint16_t)(speed * 0xFFFF));
}

// 原地右旋（所有轮对称反向）
void Move_RotateRight(float32_t speed) {
    SetMotor1Direction(FORWARD);  SetMotor1Speed((uint16_t)(speed * 0xFFFF));
    SetMotor2Direction(FORWARD);  SetMotor2Speed((uint16_t)(speed * 0xFFFF));
    SetMotor3Direction(BACKWARD); SetMotor3Speed((uint16_t)(speed * 0xFFFF));
    SetMotor4Direction(BACKWARD); SetMotor4Speed((uint16_t)(speed * 0xFFFF));
}

// 停止
void Move_Stop(void) {
    SetMotor1Speed(0);
    SetMotor2Speed(0);
    SetMotor3Speed(0);
    SetMotor4Speed(0);
} 